/*
 * ServoMotor.c
 *
 *  Created on: Nov 21, 2013
 *      Author: Administrador
 */


#include "Mytypes.h"
#include "ServoMotor.h"
#include "PWM_Config.h"

//Function Initial
void vdfn_ServoInit(void){
	
	//Parameters
	Servo_Reference = 40;	//Degree
	Servo_Past = 0;
	Servo_Error = 0;
	Servo_Kp = 1;
	
	// System Clock Gating (Enable)
	SIM_SCGC6 |= SIM_SCGC6_TPM1_MASK;
	
	//Preescaler
	TPM1_SC |= TPM_SC_PS(3);
	
	//Select the Counter max value 100% or 0xFFFF
	TPM1_MOD = PWM_20ms_PERIOD;	//50 Hz Frecuency
	
	//Configures the PWM SERVO (No interrupt)
	TPM1_C0SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	//For Center Aligned PWM
	TPM1_C1SC |=  TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK;	
	
	//Configure the initial value for the counter in this case 50%
	TPM1_C0V = 3300;
	TPM1_C1V = 0x7530;
	
	//Enable the Counter
	TPM1_SC |= TPM_SC_CMOD(1);
	
	
}


void u16fn_ServoControl(uint16 Eyed_Value){
	
	Servo_Vector = (Eyed_Value*4)/10;	//Value in Degree
	
	//Version PRO
	if(Eyed_Value < 50 || Eyed_Value > 150 ){
			Reference = 10;					}
		else{
			Reference = 30;		
		}
	
	///Coleteo al centro
	/*
	if(Eyed_Value < 20 || Eyed_Value > 180 ){
			Servo_Kp = 2; 
		}
		else{
			Servo_Kp = 1;
		}
	*/
	
	//CONTROL
	
	Servo_Error = Servo_Reference - Servo_Vector;	//Calculate Error
	
	//Controler New Value
	Servo_Controller = Servo_Reference - (Servo_Error * Servo_Kp); 
	
	//Security 
	if(Servo_Controller > 80){Servo_Controller = 80; }
	if(Servo_Controller < 2 ){Servo_Controller = 0; }
	

		
	//Variacion de referencia del servo para las curvas
	/*
	if(Eyed_Value < 20){
		Servo_Reference = 70;
	}
	else if(Eyed_Value > 180){
		Servo_Reference = 10;
	}
	else{
		Servo_Reference = 40;
	}
	*/

	
	
	/*
	if(Eyed_Value < 50 || Eyed_Value > 150 ){
		u16fn_ServoMovement_Slow(Servo_Controller);
	}
	else{
		u16fn_ServoMovement_Fast(Servo_Controller);
	}
	*/
	
	
	
	//Parte de la seguridad por si se aleja bastante de la linea
	//if(Eyed_Value < 20 || Eyed_Value > 180 ){Servo_Kp = 2;}
	//else{Servo_Kp = 1;}
	
	//Parte de la seguridad por si se aleja bastante de la linea
	
	//if(Servo_Controller < 50 && Servo_Controller > 30){Servo_Controller = 40;}
	
	//Servo_Past = Servo_Controller;
	/*
	if(Servo_Kp == 3){
	u16fn_ServoMovement_Slow(Servo_Controller);}
	else{
		u16fn_ServoMovement_Fast(Servo_Controller);
	}
	*/

	u16fn_ServoMovement_Slow(Servo_Controller);
}

void u16fn_ServoMovement_Slow(uint16 Servo_Degree){
	
	Servo_Value = (Servo_Degree * 275) + 22000;
	
	Servo_Value = Servo_Value/10;
		
	TPM1_C0V = Servo_Value;

}

void u16fn_ServoMovement_Fast(uint16 Servo_Degree){
	
	Servo_Value = (Servo_Degree * 125) + 28000;
	
	Servo_Value = Servo_Value/10;
		
	TPM1_C0V = Servo_Value;

}

/* TEST SERVO */
void vdfn_ServoTest(){
	
	Servo_Value_Test = Servo_Value_Test - 5;
	
	
	if(Servo_Value_Test < 5){
		
		Servo_Value_Test = 80;
	}
	
	TPM1_C0V = (Servo_Value_Test* 22) + 2400;
	
}



